package de.koller.aframe.util.math.interpolation;

import de.koller.aframe.util.math.Function2DPreview;
import de.koller.aframe.util.math.IFunction2D;

public class CubicInterpolation {
	
	public static float cubic( float x, float... v ) {
		return new CubicInterpolator().getValue( v, x );
	}
	
	public static float cubicDerivate( float x, float... v ) {
		return new CubicInterpolator().getDerivate( v, x );
	}
	
	public static float bicubic( float x, float y, float... v ) {
		BicubicInterpolator b = new BicubicInterpolator();
		float[][] p = new float[4][4];
		p[0][0] = v[ 0]; p[1][0] = v[ 1]; p[2][0] = v[ 2]; p[3][0] = v[ 3];
		p[0][1] = v[ 4]; p[1][1] = v[ 5]; p[2][1] = v[ 6]; p[3][1] = v[ 7];
		p[0][2] = v[ 8]; p[1][2] = v[ 9]; p[2][2] = v[10]; p[3][2] = v[11];
		p[0][3] = v[12]; p[1][3] = v[13]; p[2][3] = v[14]; p[3][3] = v[15];
		return b.getValue( p, x, y );
	}
	
	public static float tricubic( float x, float y, float z, 
			int offsetX, int offsetY, int offsetZ, float[][][] v ) {
		TricubicInterpolator b = new TricubicInterpolator();
		
		float[][][] p = new float[4][4][4];
		for( int ax = 0; ax < 4; ax++ )
			for( int ay = 0; ay < 4; ay++ )
				for( int az = 0; az < 4; az++ )
					p[ax][ay][az] = v[offsetX+ax][offsetY+ay][offsetZ+az];
					
		return b.getValue( p, x, y, z );
	}
	
	private static class CubicInterpolator {
		public float getValue ( float[] p, float x ) {
			return p[1] + .5f * x*(p[2] - p[0] + x*(2.f*p[0] - 5.f*p[1] + 4.f*p[2] - p[3] + x*(3.f*(p[1] - p[2]) + p[3] - p[0])));
		}
		
		public float getDerivate( float[] p, float x ) {
			float k4 = p[1];
			float k3 = p[2] - p[0];
			float k2 = 2f*p[0] - 5f*p[1] + 4f*p[2] - p[3];
			float k1 = 3f*(p[1]-p[2]) + p[3] - p[0];
			return .5f * (k3 + x*( 2f*k2 + 3*k1*x ));
			//return (3f/2f) * (3f*(p[1]-p[2]) + p[3]-p[0])*x*x + (2f*p[0]-5f*p[1]+4f*p[2]-p[3])* x + (1f/2f)*(p[2]-p[0]);
		}
	}

	private static class BicubicInterpolator extends CubicInterpolator {
		private float[] arr = new float[4];

		public float getValue (float[][] p, float x, float y) {
			arr[0] = getValue( p[0], y );
			arr[1] = getValue( p[1], y );
			arr[2] = getValue( p[2], y );
			arr[3] = getValue( p[3], y );
			return getValue( arr, x );
		}
		
	}

	private static class TricubicInterpolator extends BicubicInterpolator {
		private float[] arr = new float[4];

		public float getValue (float[][][] p, float x, float y, float z) {
			arr[0] = getValue(p[0], y, z);
			arr[1] = getValue(p[1], y, z);
			arr[2] = getValue(p[2], y, z);
			arr[3] = getValue(p[3], y, z);
		return getValue(arr, x);
	}
		
}

	public static void main( String[] args ) {
		IFunction2D t = new CubicInterpolation.TestFunction();
		Function2DPreview.showFunction( 0, 0, 1, 1, t );
		IFunction2D t2 = new CubicInterpolation.TestFunction2();
		Function2DPreview.showFunction( 0, 0, 1, 1, t2 );
		//t.getValue( .2f, .3f );
	}
	
	private static class TestFunction implements IFunction2D {

		@Override
		public float getValue( float x, float y ) {
			return (float)CubicInterpolation.bicubic( x, y,
				1, 1, 1, 1,
				1, 1, 0, 1,
				1, 0, 1, 1,
				1, 1, 0, 0 );
		}
		
	}
	
	private static class TestFunction2 implements IFunction2D {

		@Override
		public float getValue( float x, float y ) {
			return (float)CubicInterpolation.bicubic( x, y,
				1, 1, 1, 1,
				1, 0, 1, 0,
				0, 1, 1, 0,
				1, 0, 0, 1 );
		}
		
	}
	
}
